#include "bsp/bsp.h"
#include "board.h"
#include "os/types.h"
#include "utils/utils.h"
#include <string.h>

#define one_page_size CONFIG_FLASH_PAGE_SIZE

static void flash_clear_error(void) {
	flash_flag_clear(FLASH_OBF_FLAG | FLASH_ODF_FLAG | FLASH_PRGMERR_FLAG | FLASH_EPPERR_FLAG);
}

// @brief Erases a flash sector. This sets all bits in the sector to 1.
// The sector's current index is reset to the minimum value (n_reserved).
// @returns 0 on success or a non-zero error code otherwise
int flash_erase_page(u32 address) {
	wdog_reload();
	flash_unlock();
	flash_clear_error();
	flash_status_type result = flash_sector_erase(address);
	flash_lock();
    return (FLASH_OPERATE_DONE == result)?0:-1;
}

int flash_erase_pages(u32 address, s32 len) {
  	uint32_t pages = (len + one_page_size - 1)/one_page_size;
	for (int page = 0; page < pages; page++) {
		flash_erase_page(address + page * one_page_size);
	}
	return 0;
}


int flash_write_page(u32 addr, uint8_t *data, u32 len, bool erase) {
    if (erase && flash_erase_page(addr) != 0) {
        return -1;
    }
	wdog_reload();
	flash_unlock();
	int total_words = len / 4;
	uint32_t *p_u32_data = (uint32_t *)data;
	int i;
	for (i = 0; i < total_words; i++){
		flash_clear_error();
		flash_word_program(addr, p_u32_data[i]);
		data += 4;
		addr += 4;
	}

	int remain_len = len - total_words * 4;
	if (remain_len > 0){
		uint32_t words = 0;
		for (int i = 0; i < remain_len; i++){
			words |= data[i] << (8*i);
		}
		flash_clear_error();
		flash_word_program(addr, words);
	}
	flash_lock();
    return 0;
}

